/*
 * @Author: huangyupei huangyupei2021@ia.ac.cn
 * @Date: 2024-09-20 13:06:00
 * @LastEditors: huangyupei huangyupei2021@ia.ac.cn
 * @LastEditTime: 2025-03-19 23:00:50
 * @FilePath: /SelfCalib_OptiAcoustic/include/camera_pose_estimation.h
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#pragma once

#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include "Thirdparty/g2o/g2o/core/base_vertex.h"
#include "Thirdparty/g2o/g2o/core/base_binary_edge.h"
#include "Thirdparty/g2o/g2o/core/block_solver.h"
#include "Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h"
#include "Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h"
#include "Thirdparty/g2o/g2o/core/robust_kernel_impl.h"
#include "Thirdparty/g2o/g2o/solvers/linear_solver_dense.h"
#include "Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h"
#include "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h"
#include "Thirdparty/g2o/g2o/types/types_sba.h"

#include "sophus/se3.hpp"


using namespace std;
using namespace cv;
using namespace Eigen;

// void find_feature_matches(
//   const Mat &img_1, const Mat &img_2,
//   std::vector<KeyPoint> &keypoints_1,
//   std::vector<KeyPoint> &keypoints_2,
//   std::vector<int> &matches);

// 2D-2D 对极几何求解
void camera_pose_estimation_2d2d(
  std::vector<KeyPoint> keypoints_1,
  std::vector<KeyPoint> keypoints_2,
  std::vector<int> matches,
  Mat &R, Mat &t);

// 像素坐标转相机归一化坐标
Point2d pixel2cam(const Point2d &p, const Mat &K);

//-- 验证E=t^R*scale，验证对极约束
void validateEpipolar(Mat R, Mat t, const vector<cv::KeyPoint> vKeys1, const vector<cv::KeyPoint> vKeys2, vector<int> matches);

// 三角化函数
void triangulationCamera(const vector<cv::KeyPoint> &kp_1, const vector<cv::KeyPoint> &kp_2, const vector<int> &matches,
                         const Mat &R, const Mat &t, vector<Eigen::Vector3d> &points);


// G2O BA中使用的顶点
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    // 重置(初始化)
    virtual void setToOriginImpl() override {   // 重写基类虚函数
        _estimate = Sophus::SE3d();
    }

    // 自定义更新方式：SE3李代数中的左乘
    virtual void oplusImpl(const double* update) override {
        Eigen::Matrix<double, 6, 1> update_eigen;
        update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
        _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
    }

    virtual bool read(istream &in) override {}

    virtual bool write(ostream &out) const override {}
}; 


// 三维点顶点类型, 用于相机恢复的三维点或者声呐BA恢复的三维点
class VertexPoint: public g2o::BaseVertex<3, Vector3d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    VertexPoint() {}
    virtual void setToOriginImpl() override {
        _estimate = Vector3d(0, 0, 0);
    }

    virtual void oplusImpl(const double *update_) override {
        Eigen::Map<const Eigen::Vector3d> update(update_);
        _estimate += update;
    }

    virtual bool read(istream &in) {}
    virtual bool write(ostream &out) const {}
};


// G2O BA中使用的边
class EdgeProjection: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    
    EdgeProjection(const Vector3d& pos, const Matrix3d& K): _pos3d(pos), _K(K){}

    // 计算边的误差
    virtual void computeError() override {
        const VertexPose* v = static_cast<VertexPose*>(_vertices[0]);
        Sophus::SE3d T = v->estimate();
        Vector3d pos_pixel = _K * (T * _pos3d);     // 3D点投影至像素平面
        pos_pixel /= pos_pixel[2];                  // 转换为归一化坐标
        _error = _measurement - pos_pixel.head<2>();    // 计算边的误差
    }

    // 使用数值求导，暂时不计算雅克比矩阵
    
    virtual bool read(istream &in) override {}

    virtual bool write(ostream &out) const override {}
    
private:
    Vector3d _pos3d;
    Matrix3d _K;
};


inline Vector2d CameraProjection(Vector3d P_ci_j) {
    Eigen::Matrix3d K;
    // double focal_length = 335;
    // K << focal_length, 0, 320,
    //            0, focal_length, 240,
    //            0, 0, 1;
    double focal_length = 1200;
    K << focal_length, 0, 960,
         0, focal_length, 540,
         0, 0, 1;

    Vector3d pos_pixel = K * P_ci_j;        // 3D点投影至像素平面
    pos_pixel /= pos_pixel[2];              // 转换为归一化坐标
    return pos_pixel.head<2>();
}

class EdgeLocalCamBAProjection: public g2o::BaseBinaryEdge<2, Eigen::Vector2d, VertexPose, VertexPoint> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    
    // 计算边的误差
    virtual void computeError() override {
        auto v0 = (VertexPose *) _vertices[0];
        auto v1 = (VertexPoint *) _vertices[1];
        Sophus::SE3d T_ci_c0 = v0->estimate();
        Vector3d P_c0_j = v1->estimate();

        Vector3d P_ci_j = T_ci_c0 * P_c0_j;
        Vector2d proj = CameraProjection(P_ci_j);
        _error = _measurement - proj;

    }

    // 使用数值求导，暂时不计算雅克比矩阵
    
    virtual bool read(istream &in) override {}

    virtual bool write(ostream &out) const override {}

};



// typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
// typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;

// 3D-2D PnP求解 by G2O
void camera_pose_estimation_3d2d(const vector<Vector3d>& pts_3d, const vector<Vector2d>& pts_2d, Sophus::SE3d& pose);

/**
 * @brief 相机的局部BA函数，待优化的是三角化得到的3D点、相机位姿序列T_c1_ci，观测量是每个位姿处的相机2D测量
 * 
 * @param[in] vPointsCamera
 * @param[in] vCameraPoses 
 * @param[in] vCameraMeasAll 
 */
void LocalCamBA(vector<Vector3d> &vPointsCamera, vector<Sophus::SE3d> &vCameraPoses, vector<vector<Vector2d>> vCameraMeasAll);